#include <stdio.h>
#include "glCamera.h"
#include "math_funcs.h"

glCamera::glCamera()
{
	// Constructor
    // Initalize all our member variables.
    m_MaxYawRate			= 0.0f;
    m_MaxPitchRate			= 0.0f;
	m_MaxRollRate			= 0.0f;

    m_YawDegrees			= 0.0f;
    m_PitchDegrees			= 0.0f;
    m_RollDegrees			= 0.0f;

    m_MaxForwardVelocity	= 0.0f;
    m_ForwardVelocity		= 0.0f;
}

glCamera::~glCamera() {
	// Destructor
}

void glCamera::SetPerspective()
{
    GLfloat Matrix[16];
    glQuaternion q;

    // create the quaternions that will represent our rotations
    m_qPitch.CreateFromAxisAngle(1.0f, 0.0f, 0.0f, m_PitchDegrees);
    m_qYaw.CreateFromAxisAngle(0.0f, 1.0f, 0.0f, m_YawDegrees);
    m_qRoll.CreateFromAxisAngle(0.0f, 0.0f, 1.0f, m_RollDegrees);




    // Combine the pitch and Yaw rotations and store the results in q
//    q = m_qPitch * m_qYaw * m_qRoll * q;
    q = m_qPitch * q;
    q = q * m_qYaw;
    q = m_qRoll * q;
//    q = m_qRoll * q;


//	q.EulerToQuat(m_PitchDegrees, m_YawDegrees, m_RollDegrees);
	q.CreateMatrix(Matrix);

	// Let OpenGL set our new perspective on the world!
//	glMultMatrixf(Matrix);

	glVector axis;
	float angle;
	q.GetAxisAngle(axis, angle);
	angle = RAD2DEG(angle);
	glRotatef(angle, axis.i, axis.j, axis.k);

	// Create a matrix from the pitch Quaternion and get the j vector 
	// for our direction.
	m_qPitch.CreateMatrix(Matrix);
	m_DirectionVector.j = Matrix[9];

	// Combine the Yaw and pitch rotations and make a matrix to get
	// the i and k vectors for our direction.
    q = m_qYaw * m_qPitch;

    q.CreateMatrix(Matrix);
    m_DirectionVector.i = Matrix[8];
    m_DirectionVector.k = Matrix[10];

//	printf("in camera: %f %f %f\n",m_DirectionVector.i,m_DirectionVector.j,m_DirectionVector.k);

/*
    // Scale the direction by our speed.
    m_DirectionVector *= m_ForwardVelocity;

    // Increment our position by the vector
    m_Position.x += m_DirectionVector.i;
    m_Position.y += m_DirectionVector.j;
    m_Position.z += m_DirectionVector.k;

    // Translate to our new position.
    glTranslatef(-m_Position.x, -m_Position.y, m_Position.z);
*/

	// store current direction
	direction.i = Matrix[8];
	direction.j = Matrix[9];
	direction.k = Matrix[10];

}

void glCamera::ChangeYaw(GLfloat degrees) {
    if(fabs(degrees) < fabs(m_MaxYawRate))
    {
		// Our Yaw is less than the max Yaw rate that we 
		// defined so lets increment it but first we must check
		// to see if we are inverted so that our Yaw will not
		// become inverted.
		if(m_PitchDegrees > 90 && m_PitchDegrees < 270
		   || (m_PitchDegrees < -90 && m_PitchDegrees > -270))
		{
			m_YawDegrees -= degrees;
		}
		else
		{
			m_YawDegrees += degrees;
		}
    }
    else
    {
		// Our Yaw is greater than the max Yaw rate that
		// we defined so we can only increment our Yaw by the 
		// maximum allowed value.
		if(degrees < 0)
		{
			// Check to see if we are upside down.
			if((m_PitchDegrees > 90 && m_PitchDegrees < 270)
			   || (m_PitchDegrees < -90 && m_PitchDegrees > -270))
			{
			// Ok we would normally decrement here but since we are upside
			// down then we need to increment our Yaw
			m_YawDegrees += m_MaxYawRate;
			}
			else
			{
			// We are not upside down so decrement as usual
			m_YawDegrees -= m_MaxYawRate;
			}
		}
		else
		{
			// Check to see if we are upside down.
			if(m_PitchDegrees > 90 && m_PitchDegrees < 270
			   || (m_PitchDegrees < -90 && m_PitchDegrees > -270))
			{
			// Ok we would normally increment here but since we are upside
			// down then we need to decrement our Yaw.
			m_YawDegrees -= m_MaxYawRate;
			}
			else
			{
			// We are not upside down so increment as usual.
			m_YawDegrees += m_MaxYawRate;
			}
		}
    }
	
    // We don't want our Yaw to run away from us either. Although it
    // really doesn't matter I prefer to have my Yaw degrees
    // within the range of -360.0f to 360.0f
    if(m_YawDegrees > 360.0f)
    {
    	m_YawDegrees -= 360.0f;
    }
	else if(m_YawDegrees < -360.0f)
    {
		m_YawDegrees += 360.0f;
    }
}

void glCamera::ChangePitch(GLfloat degrees) {
    if(fabs(degrees) < fabs(m_MaxPitchRate))
    {
	// Our pitch is less than the max pitch rate that we 
	// defined so lets increment it.
    	m_PitchDegrees += degrees;
    }
    else
    {
	// Our pitch is greater than the max pitch rate that
	// we defined so we can only increment our pitch by the 
	// maximum allowed value.
	if(degrees < 0)
	{
	    // We are pitching down so decrement
		m_PitchDegrees -= m_MaxPitchRate;
	}
	else
	{
		// We are pitching up so increment
		m_PitchDegrees += m_MaxPitchRate;
	}
    }
    // We don't want our pitch to run away from us. Although it
    // really doesn't matter I prefer to have my pitch degrees
    // within the range of -360.0f to 360.0f
    if(m_PitchDegrees > 360.0f)
    {
    	m_PitchDegrees -= 360.0f;
    }
    else if(m_PitchDegrees < -360.0f)
    {
    	m_PitchDegrees += 360.0f;
    }
}

void glCamera::ChangeRoll(GLfloat degrees) {
    if(fabs(degrees) < fabs(m_MaxRollRate))
    {
	// Our pitch is less than the max pitch rate that we
	// defined so lets increment it.
    	m_RollDegrees += degrees;
    }
    else
    {
	// Our pitch is greater than the max pitch rate that
	// we defined so we can only increment our pitch by the
	// maximum allowed value.
	if(degrees < 0)
	{
	    // We are pitching down so decrement
		m_RollDegrees -= m_MaxRollRate;
	}
	else
	{
		// We are pitching up so increment
		m_RollDegrees += m_MaxRollRate;
	}
    }
    // We don't want our pitch to run away from us. Although it
    // really doesn't matter I prefer to have my pitch degrees
    // within the range of -360.0f to 360.0f
    if(m_RollDegrees > 360.0f)
    {
    	m_RollDegrees -= 360.0f;
    }
    else if(m_RollDegrees < -360.0f)
    {
    	m_RollDegrees += 360.0f;
    }
}

glVector glCamera::GetDirection() {
	
	return direction;
}


